import numpy as np
import Common.MatrixProcess as mp


class AdaptiveAdmittanceControl():
    def __init__(self, k3, b3, m3, f3):
        self.k = k3
        self.b = b3
        self.m = m3

        self.goalFt = f3  # 工具坐标系下的f3

        self.phi = np.zeros((3))  # 自适应调整参数
        self.alpha = 0.5
        self.Fe_t0 = np.zeros((3))
        self.Fe_t1 = np.zeros((3))

        self.delta_pose_t0 = np.zeros((3))
        self.delta_vel_t0 = np.zeros((3))
        self.delta_acc_t0 = np.zeros((3))

        self.delta_pose_t1 = np.zeros((3))
        self.delta_vel_t1 = np.zeros((3))
        self.delta_acc_t1 = np.zeros((3))

    def calpose(self, curpose, goalpose, curvel, curforce):
        """

        Args:
            curpose:当前位置，从机器人读取上来的
            goalpose: 目标轨迹，世界坐标系下的
            curvel:
            curforce:

        Returns:

        """
        # 1 工具坐标系下实际受力（外界对机器人）与理想作用力（通过机器人作用给外界）差值
        curforce[np.abs(curforce) < 2] = 0  # 滤去小于2N的方向力
        self.Fe_t1 = curforce[:3] - self.goalFt
        # 传感器上的力转换为基坐标上
        # RTcp_in_base = mp.create_transformation_matrix(curpose)[:3, :3]
        # Fe = np.reshape(Fe, (3, 1))
        # Fe = RTcp_in_base.dot(Fe)
        # Fe = np.reshape(Fe,(1,3))
        # Fe=np.array([0,0,0])
        # Ve=curvel[:3]#实际速度与理想速度之差
        # Xe=curpose[:3]-goalpose[:3]#实际位置与理想位置之差

        # 2 计算阻尼自适应修正
        Ve = self.delta_vel_t0
        Xe = self.delta_pose_t0
        phi_new = self.phi + self.alpha / self.b * (-self.Fe_t0)
        b_delta = self.b * phi_new / (Ve + 10e-8)
        # 3 导纳公式
        self.delta_acc_t1 = (1 / self.m) * (self.Fe_t1 - (self.b+b_delta) * Ve - self.k * Xe)  # 求加速度的差值
        self.delta_vel_t1 = Ve + self.delta_acc_t1 * 0.008  # UR5e的控制频率

        sump = np.linalg.norm(self.delta_vel_t1)
        if sump > 0.06:
            self.delta_vel_t1 = (0.0001 / sump) * self.delta_vel_t1

        self.delta_pose_t1 = Xe + self.delta_vel_t1 * 0.008  # x0+vt

        self.delta_pose_t0 = self.delta_pose_t1
        self.delta_vel_t0 = self.delta_vel_t1
        self.Fe_t0 = self.Fe_t1
        self.phi = phi_new
        # 坐标变换
        # RBase_Tcp = mp.create_transformation_matrix(curpose)[:3, :3]
        # deltapose = np.reshape(self.delta_pose_t1, (3, 1))
        # deltapose = RBase_Tcp.dot(deltapose)
        # deltapose = np.reshape(deltapose, (1, 3))

        movepose = np.zeros(6)
        movepose[:3] = self.delta_pose_t1 + goalpose[:3]
        print("delta_pose_t1=", self.delta_pose_t1)
        movepose[3:] = curpose[3:]
        return movepose  # 这个是最终的坐标位置，基坐标系下的

# A=AddmittanceControl()
